package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.*;

/**
 *
 * @author Super NURDs 
 */

public class Collector extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    Talon positionMotor;
    Talon lowerBeltMotor;
    Talon upperBeltMotor;
    
    public double currentPositionSpeed = 0.0;
    public double speed = 0.0;
        
    public Collector(){
        super("Collector");
        try {
            positionMotor = new Talon(RobotMap.COLLECTOR_POSITION_TALON);
            positionMotor.setSafetyEnabled(false);
            lowerBeltMotor = new Talon(RobotMap.LOWER_COLLECTOR_BELT_TALON);
            lowerBeltMotor.setSafetyEnabled(false);
            upperBeltMotor = new Talon(RobotMap.UPPER_COLLECTOR_BELT_TALON);
            upperBeltMotor.setSafetyEnabled(false);
        }
        catch (Exception e){
            e.printStackTrace();
        }
    }
    
    public void initDefaultCommand() {
        //this.setDefaultCommand(new DoCollectorForward());
    }
    
    // positive voltage moves collector up
    public void setPositionSpeed(double s) {
        positionMotor.set(s);
        currentPositionSpeed = s;
    }
    
    public double getPositionSpeed() {
        return currentPositionSpeed;
    }
    
    public void setLowerSpeed(double s) {
        lowerBeltMotor.set(s);
        speed = s;
    }
    
    public void setUpperSpeed(double s) {
        upperBeltMotor.set(s);
        speed = s;
    }
    
    public void forward() {
        setLowerSpeed(-1.0);
        setUpperSpeed(-0.85);
    }  
    
    public void reverse() {
        setLowerSpeed(1.0);
        setUpperSpeed(0.45);
        
    }   
    
    public void stop(){
        setLowerSpeed(0.0);
        setUpperSpeed(0.0);
    }   

    public double getSpeed() {
        return speed;
    }
  }